/**
 * Simple Snowflake MEGA sumo robot unit test selection application
 * Copyright (c) 2013 Maris BECS. All rights reserved.
 */

#include <asf.h>
#include "stdio.h"
#include "sysfont.h"
#include "keyboard.h"
#include "gfx_mono_menu.h"
#include "gfx_mono_spinctrl.h"
#include "unit_tests.h"
#include "main.h"
#include "sensors.h"
#include "motors.h"
#include "system_tests.h"

// Menus
PROGMEM_DECLARE(char, tests_menu_title[]) = "# Tests(4) #";
PROGMEM_DECLARE(char, tests_menu_1[]) = "Sensor test";
PROGMEM_DECLARE(char, tests_menu_2[]) = "SYSTEM tests";
PROGMEM_DECLARE(char, tests_menu_3[]) = "Motor test";
PROGMEM_DECLARE(char, tests_menu_4[]) = "Encoders test";

PROGMEM_STRING_T tests_menu_strings[] = {
	tests_menu_1,
	tests_menu_2,
	tests_menu_3,
    tests_menu_4,
};

struct gfx_mono_menu tests_menu = {
	// Title
	tests_menu_title,
	// Array with menu strings
	tests_menu_strings,
	// Number of menu elements
	4,
	// Initial selection
	0
};

PROGMEM_DECLARE(char, tests_motor_spinner_string_pwm[]) = "PWM, % x10";
PROGMEM_DECLARE(char, tests_motor_spinner_string_time[]) = "Time, sec/5";

/**
 * \brief Set the motor PWM and drive time using a spinner widget
 */
static void motor_test_application(void)
{
	struct keyboard_event input;
	int8_t pwm_;
	int16_t time_;
	struct gfx_mono_spinctrl pwm_spinner;
	struct gfx_mono_spinctrl time_spinner;
	struct gfx_mono_spinctrl_spincollection motor_spinners;
	uint8_t spinner_status;
	int16_t spinner_results[2];

	// Prepare the spinner widget for time selection
	gfx_mono_spinctrl_init(&pwm_spinner, SPINTYPE_INTEGER, tests_motor_spinner_string_pwm, NULL, -10, 10, 0);
	gfx_mono_spinctrl_init(&time_spinner, SPINTYPE_INTEGER, tests_motor_spinner_string_time, NULL, 0, 10, 0);

	// Create motor spin collector
	gfx_mono_spinctrl_spincollection_init(&motor_spinners);
	gfx_mono_spinctrl_spincollection_add_spinner(&pwm_spinner, &motor_spinners);
	gfx_mono_spinctrl_spincollection_add_spinner(&time_spinner, &motor_spinners);

	// Get global motor test settings
	pwm_ = 1;
	time_ = Time_ms / 1000;

	// Set spinners to current settings as initial position
	pwm_spinner.integer_data = pwm_;
	time_spinner.integer_data = time_;

	gfx_mono_spinctrl_spincollection_show(&motor_spinners);

	do {
		do {
			keyboard_get_key_state(&input);
			// Wait for key release
		} while (input.type != KEYBOARD_RELEASE);
		// To exit loop at any time
		if (input.keycode == KEYBOARD_BACK){
			// return to tests menu
			return;
		};
		// Send key to spinner collection
		spinner_status = gfx_mono_spinctrl_spincollection_process_key(&motor_spinners, input.keycode, spinner_results);
	} while (spinner_status != GFX_MONO_SPINCTRL_EVENT_FINISH);

	pwm_ = spinner_results[0] * 10;
	time_ = spinner_results[1] * 1000;
	
	#ifdef DEBUG_ENABLE
		usart_serial_putchar( USART_SERIAL, pwm_ );
		usart_serial_putchar( USART_SERIAL, time_/1000 );
	#endif
	
	delay_ms(500);
		gpio_set_pin_low(NHD_C12832A1Z_BACKLIGHT);	// To test delay_ms
	drive(pwm_, pwm_);
	delay_ms(time_/5);	// wtf? something definitely wrong with the time settings: tested 10sec is ~26sec
	drive(0,0);
		gpio_set_pin_high(NHD_C12832A1Z_BACKLIGHT);
}


/**
 * \brief Test robot distance and line sensors
 */
static void robotsensor_test_application(void)
{
	struct keyboard_event input;

	// turn on power for sensors
	sensorPower(true);
	// clear screen
	gfx_mono_draw_filled_rect(0, 0, 128, 32, GFX_PIXEL_CLR);
	// Enable kill switch - press "8" on IR remote control to kill
	ioport_set_pin_low(START_SW);

	Distance_sens = sensor_read();
	Line_sens = line_read();

	#ifdef DEBUG_ENABLE
		usart_serial_putchar( USART_SERIAL, Distance_sens );
		usart_serial_putchar( USART_SERIAL, Line_sens );
	#endif


	while (true) 
	{
		keyboard_get_key_state(&input);
		// Any key press&release will exit the loop
		if (input.type == KEYBOARD_RELEASE) {
			sensorPower(false);
			// set IR module back to OFF state
			ioport_set_pin_high(START_SW);
			break;
		}

		Distance_sens = sensor_read();
		Line_sens = line_read();
		delay_ms(10);	// loop delay - not sure if needed
		
		// IR remote control state test
		if ( LOW == ioport_get_pin_level(REMOTE_START) ){
			// remote POWER_ON or STOPPED state
			gfx_mono_draw_string("S0", 0, 0, &sysfont);
			// remote STARTED state
		} else if ( HIGH == ioport_get_pin_level(REMOTE_START) ){
			gfx_mono_draw_string("S1", 0, 0, &sysfont);
		}
		if ( HIGH == ioport_get_pin_level(REMOTE_KILL) ){
			// remote POWER_ON or STARTED state
			gfx_mono_draw_string("K1", 22, 0, &sysfont);
			} else if ( LOW == ioport_get_pin_level(REMOTE_KILL) ){
			// remote STOPPED state
			gfx_mono_draw_string("K0", 22, 0, &sysfont);
		}
		// internal kill switch state test
		if ( HIGH == ioport_get_pin_level(KILL_SW) ){
			gfx_mono_draw_string("BK1", 0, 12, &sysfont);
			} else if ( LOW == ioport_get_pin_level(KILL_SW) ){
			gfx_mono_draw_string("BK0", 0, 12, &sysfont);
		}

		#ifdef DEBUG_ENABLE
			usart_serial_putchar( USART_SERIAL, Distance_sens );
			usart_serial_putchar( USART_SERIAL, Line_sens );
		#endif

		/* #################### Distance sensor readings #################### */
		if(Distance_sens&0b00000001) {	// R90
			gfx_mono_draw_filled_rect(122, 12, 6, 6, GFX_PIXEL_SET); 
		} else {
			gfx_mono_draw_filled_rect(122, 12, 6, 6, GFX_PIXEL_CLR);
			gfx_mono_draw_rect(122, 12, 6, 6, GFX_PIXEL_SET);
		}

		if(Distance_sens&0b00000010) {	// R45
			gfx_mono_draw_filled_rect(115, 8, 6, 6, GFX_PIXEL_SET);
		} else {
			gfx_mono_draw_filled_rect(115, 8, 6, 6, GFX_PIXEL_CLR);
			gfx_mono_draw_rect(115, 8, 6, 6, GFX_PIXEL_SET);
		}

		if(Distance_sens&0b00000100) { // R30
			gfx_mono_draw_filled_rect(108, 3, 6, 6, GFX_PIXEL_SET);
		} else {
			gfx_mono_draw_filled_rect(108, 3, 6, 6, GFX_PIXEL_CLR);
			gfx_mono_draw_rect(108, 3, 6, 6, GFX_PIXEL_SET);
		}

		if(Distance_sens&0b00001000) {	// R10
			gfx_mono_draw_filled_rect(101, 0, 6, 6, GFX_PIXEL_SET);
		} else {
			gfx_mono_draw_filled_rect(101, 0, 6, 6, GFX_PIXEL_CLR);
			gfx_mono_draw_rect(101, 0, 6, 6, GFX_PIXEL_SET);
		}

		// PEPPERL & FUCHS front sensor
		if(HIGH == ioport_get_pin_level(PEPPERL_FUCHS)) {	// CENTER
			gfx_mono_draw_filled_rect(94, 1, 6, 6, GFX_PIXEL_SET);
		} else {
			gfx_mono_draw_filled_rect(94, 1, 6, 6, GFX_PIXEL_CLR);
			gfx_mono_draw_rect(94, 1, 6, 6, GFX_PIXEL_SET);
		}

		if(Distance_sens&0b00010000) {	// L10
			gfx_mono_draw_filled_rect(87, 0, 6, 6, GFX_PIXEL_SET);
		} else {
			gfx_mono_draw_filled_rect(87, 0, 6, 6, GFX_PIXEL_CLR);
			gfx_mono_draw_rect(87, 0, 6, 6, GFX_PIXEL_SET);
		}

		if(Distance_sens&0b00100000) {	// L30
			gfx_mono_draw_filled_rect(80, 3, 6, 6, GFX_PIXEL_SET);
		} else {
			gfx_mono_draw_filled_rect(80, 3, 6, 6, GFX_PIXEL_CLR);
			gfx_mono_draw_rect(80, 3, 6, 6, GFX_PIXEL_SET);
		}

		if(Distance_sens&0b01000000) {	// L45
			gfx_mono_draw_filled_rect(73, 8, 6, 6, GFX_PIXEL_SET);
		} else {
			gfx_mono_draw_filled_rect(73, 8, 6, 6, GFX_PIXEL_CLR);
			gfx_mono_draw_rect(73, 8, 6, 6, GFX_PIXEL_SET);
		}

		if(Distance_sens&0b10000000) {	// L90
			gfx_mono_draw_filled_rect(66, 12, 6, 6, GFX_PIXEL_SET);
		} else {
			gfx_mono_draw_filled_rect(66, 12, 6, 6, GFX_PIXEL_CLR);
			gfx_mono_draw_rect(66, 12, 6, 6, GFX_PIXEL_SET);
		}


		/* #################### Line sensor readings #################### */
		if (~Line_sens&0b00000010) {	// Line_FL
			gfx_mono_draw_filled_circle(80, 19, 3, GFX_PIXEL_SET, GFX_WHOLE);
		} else {
			gfx_mono_draw_filled_circle(80, 19, 3, GFX_PIXEL_CLR, GFX_WHOLE);
			gfx_mono_draw_circle(80, 19, 3, GFX_PIXEL_SET, GFX_WHOLE);
		}

		if (~Line_sens&0b00000100) {	// Line_FR
			gfx_mono_draw_filled_circle(113, 19, 3, GFX_PIXEL_SET, GFX_WHOLE);
		} else {
			gfx_mono_draw_filled_circle(113, 19, 3, GFX_PIXEL_CLR, GFX_WHOLE);
			gfx_mono_draw_circle(113, 19, 3, GFX_PIXEL_SET, GFX_WHOLE);
		}

		if (~Line_sens&0b00001000) {	// Line_BL
			gfx_mono_draw_filled_circle(80, 28, 3, GFX_PIXEL_SET, GFX_WHOLE);
		} else {
			gfx_mono_draw_filled_circle(80, 28, 3, GFX_PIXEL_CLR, GFX_WHOLE);
			gfx_mono_draw_circle(80, 28, 3, GFX_PIXEL_SET, GFX_WHOLE);
		}

		if (~Line_sens&0b00000001) {	// Line_BR
			gfx_mono_draw_filled_circle(113, 28, 3, GFX_PIXEL_SET, GFX_WHOLE);
		} else {
			gfx_mono_draw_filled_circle(113, 28, 3, GFX_PIXEL_CLR, GFX_WHOLE);
			gfx_mono_draw_circle(113, 28, 3, GFX_PIXEL_SET, GFX_WHOLE);
		}

	}
}


/**
 * \brief Test robot encoder sensors
 */
static void encoder_test_application(void)
{
	struct keyboard_event input;
	
	// Clear screen
	gfx_mono_draw_filled_rect(0, 0, 128, 32, GFX_PIXEL_CLR);

	while (true) {
		keyboard_get_key_state(&input);
		// Any key press&release will return to tests menu
		if (input.type == KEYBOARD_RELEASE) {
			return;
		}

		delay_ms(5);	// while loop delay

		// left encoder state test
		if ( LOW == ioport_get_pin_level(ENCODER_L) ){
			gfx_mono_draw_string("L0", 90, 0, &sysfont);
		} else if ( HIGH == ioport_get_pin_level(ENCODER_L) ){
			gfx_mono_draw_string("L1", 90, 0, &sysfont);
		}
		// right encoder state test
		if ( LOW == ioport_get_pin_level(ENCODER_R) ){
			gfx_mono_draw_string("R0", 110, 0, &sysfont);
		} else if ( HIGH == ioport_get_pin_level(ENCODER_R) ){
			gfx_mono_draw_string("R1", 110, 0, &sysfont);
		}
	}
}	


/**
 * \brief Show a menu presenting a different unit test applications
 */
void unit_tests_application(void)
{	
	uint8_t menu_status;
	struct keyboard_event input;

	while(true) {
		gfx_mono_menu_init(&tests_menu);
		do {
			do {
				keyboard_get_key_state(&input);
				// Wait for key release
			} while (input.type != KEYBOARD_RELEASE);

			// Send key to menu system
			menu_status = gfx_mono_menu_process_key(&tests_menu,
					input.keycode);
			// Wait for something useful to happen in the menu system
		} while (menu_status == GFX_MONO_MENU_EVENT_IDLE);

		switch(menu_status) {
		case 0:
			robotsensor_test_application();
			break;
		case 1:
		    system_tests_application();
		    break;
		case 2:
			motor_test_application();
			break;
		case 3:
		    encoder_test_application();
		    break;
		default:
			// Return to the main menu on unknown element or back key
			return;
		}
	}
}
